import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

import lejos.pc.comm.NXTConnector;
import robotAPI.*;

public class RobotListener extends Thread {
	ParseXMLString parser = new ParseXMLString();

	ThreadCommunication comm;

	public RobotListener(ThreadCommunication comm) {
		this.comm = comm;
	}

	public void automatize(DataOutputStream dos, DataInputStream dis) {
		int counter;
		for (int walls = 0; walls < 6; walls++) {
			counter = 0;
			while (true) {
				try {
					if (counter == 0 && walls != 0)
						dos.writeUTF("<s2r><move req=\"-5\"/><scan req=\"12\"/><rotate req=\"90\"/></s2r>");
					else
						dos.writeUTF("<s2r><scan req=\"12\"/><move req=\"10\"/></s2r>");
					dos.flush();
				} catch (IOException e) {
					System.out.println("Failed to send message:" + e);
				}
				try {
					System.out.println("Receiving");
					String newReply = dis.readUTF();
					System.out.println(newReply);
					RobotReplyMessage msg = parser.parse(newReply);
					comm.addRobotReplyMessage(msg);
					if (msg.getScanValueN(0) <= 25) {
						break;
					}
				} catch (IOException e) {
					System.out.println("Failed to read message:" + e);
				}
				counter++;
			}
		}
	}

	public void run() {
		NXTConnector link;
		boolean connected = false;
		link = new NXTConnector();
		while (true) {
			connected = false;
			while (!connected) {
//				connected = link.connectTo("btspp://001653180896");
				connected = link.connectTo("btspp://");
				if (connected)
					System.out.println("Conntected to NXT.");
				else {
					System.out.println("Failed to connect to NXT.");
					try {
						Thread.sleep(1500);
					} catch (InterruptedException e) {
						System.out.println("Interrupted: " + e);
					}
				}
			}
			DataOutputStream dos = new DataOutputStream(link.getOutputStream());
			DataInputStream dis = new DataInputStream(link.getInputStream());
			RobotRequestMessage newMsg = null;
			while (true) {
				try {
					while (newMsg == null)
						newMsg = comm.getRobotRequestMessage();
					if (newMsg != null) {
						dos.writeUTF(newMsg.getXML());
						System.out.println("Forwarding client message: \n"
								+ newMsg.getXML());
					}
					dos.flush();
				} catch (IOException e) {
					System.out.println("Failed to send message:" + e);
				}
				try {
					System.out.println("Receiving");
					String newReply = dis.readUTF();
					System.out.println(newReply);
					comm.addRobotReplyMessage(parser.parse(newReply));
				} catch (IOException e) {
					System.out.println("Failed to read message:" + e);
					try {
						link.close();
					} catch (IOException e1) {
						// TODO Auto-generated catch block
						e1.printStackTrace();
					}
					break;
				}
				if (newMsg.automatic())
					automatize(dos, dis);
				newMsg = null;

			}
		}
	}
}